#include "Main.h"

extern std::list<Triangle> listTriangle;

Collision::Collision(){
	m_nMaxIntersections = 100;
	m_nMaxIterations = 30;
	m_pIntersections = new CollIntersect[m_nMaxIntersections];
	m_strCollisionType = "";
}

Collision::~Collision(){
	if(m_pIntersections) delete []m_pIntersections;
}

bool Collision::CollideEllipsoid(const Vector &Center, const Vector &Radius, const Vector &Velocity,Vector &NewCenter, Vector &IntegrationVelocity){
	Vector vecOutputPos, vecOutputVelocity,InvRadius, vecNormal;
	Vector eVelocity, eInputVelocity, eFrom, eTo;
	ULONG IntersectionCount,i;
	float fDistance;
	bool bHit = false;

	vecOutputPos = VectorAddition(&Center, &Velocity);
	vecOutputVelocity = Velocity;

	InvRadius.x = 1.0f/Radius.x;
	InvRadius.y = 1.0f/Radius.y;
	InvRadius.z = 1.0f/Radius.z;

	eVelocity = VectorScale(Velocity, InvRadius);
	eFrom = VectorScale(Center, InvRadius);
	eTo = VectorAddition(&eFrom, &eVelocity);

	eInputVelocity = eVelocity;

	for(i=0;i<m_nMaxIterations;++i){
		if(VectorLength(eVelocity) <1e-5f) break;
		if(EllipsoidIntersectScene(eFrom,Radius, eVelocity,m_pIntersections,IntersectionCount)){
			CollIntersect &FirstIntersect = m_pIntersections[0];

			eFrom = FirstIntersect.NewCenter;

			fDistance = Matrix3DotProduct(&(VectorSubtraction(&eTo,&FirstIntersect.NewCenter)),&FirstIntersect.IntersectNormal);
			eTo = VectorSubtraction(&eTo, &(VectorMultiplication(&FirstIntersect.IntersectNormal,fDistance)));

			eVelocity = VectorSubtraction(&eTo,&eFrom);

			Normalize(&(VectorScale(FirstIntersect.IntersectNormal,InvRadius)),&vecNormal);
			fDistance = Matrix3DotProduct(&vecOutputVelocity,&vecNormal);
			vecOutputVelocity = VectorSubtraction(&vecOutputVelocity,&(VectorMultiplication(&vecNormal,fDistance)));
			
			bHit = true;

			if(Matrix3DotProduct(&eVelocity,&eInputVelocity)<0){
				eTo = eFrom;
				break;
			}

		}else{
			break;
		}
	}
	if(bHit){
		if(i <m_nMaxIterations){
			vecOutputPos = VectorScale(eTo,Radius);
		}else{
			eFrom = VectorScale(Center, InvRadius);

			IntersectionCount = 0;
			EllipsoidIntersectScene(eFrom, Radius, eInputVelocity,m_pIntersections, IntersectionCount);
			vecOutputPos = VectorScale(m_pIntersections[0].NewCenter, Radius);

		}
	}

	NewCenter = vecOutputPos;
	IntegrationVelocity = vecOutputVelocity;

	return bHit;
}

bool Collision::EllipsoidIntersectScene(const Vector &Center,const Vector &Radius, const Vector &Velocity,CollIntersect Intersections[], ULONG &IntersectionCount){
	Vector eCenter, eVelocity, InvRadius;
	Vector ePoints[3];
	Vector eIntersectNormal, eNewCenter;
	ULONG Counter, NewIndex, FirstIndex;

	bool AddToList;
	float eInterval;
	ULONG i;

	InvRadius.x = 1.0f/Radius.x;
	InvRadius.y = 1.0f/Radius.y;
	InvRadius.z = 1.0f/Radius.z;
	
	eCenter = Center;
	eVelocity = Velocity;

	eInterval = 1.0f;

	IntersectionCount = 0;

	std::list<Triangle>::iterator Iterator = listTriangle.begin();
	for(Counter = 0;Iterator != listTriangle.end();++Iterator,++Counter){
		Triangle *pTriangle = &(*Iterator);
		if(!pTriangle) continue;
		
		ePoints[0] = pTriangle->v1;
		ePoints[1] = pTriangle->v2;
		ePoints[2] = pTriangle->v3;

		//eNormal = VectorComputeNormal(&(ePoints[0]),&(ePoints[1]),&(ePoints[2]));
		//eNormal = VectorScale(eNormal,Radius);
		//Normalize(&eNormal, &eNormal);
		if(SphereIntersectTriangle(eCenter,1.0f,eVelocity,*pTriangle,eInterval,eIntersectNormal)){
			if(eInterval>0)
				eNewCenter = VectorAddition(&eCenter,&(VectorMultiplication(&eVelocity,eInterval)));
			else
				eNewCenter = VectorSubtraction(&eCenter,&(VectorMultiplication(&eVelocity,eInterval)));
			AddToList = false;
			if(IntersectionCount == 0 || eInterval < Intersections[0].Interval){
				AddToList = true;
				NewIndex = 0;
				IntersectionCount = 1;
			}
			else if(fabsf(eInterval - Intersections[0].Interval)<1e-5f){
				if(IntersectionCount < m_nMaxIntersections){
					AddToList = true;
					NewIndex = IntersectionCount;
					IntersectionCount++;
				}
			}
			if(AddToList){
				Intersections[NewIndex].Interval = eInterval;
				Intersections[NewIndex].NewCenter = VectorAddition(&eNewCenter,&(VectorMultiplication(&eIntersectNormal,1e-3f)));
				Intersections[NewIndex].IntersectPoint = VectorSubtraction(&eNewCenter,&eIntersectNormal);
				Intersections[NewIndex].IntersectNormal = eIntersectNormal;
				Intersections[NewIndex].TriangleIndex = Counter;
			}
		}
	}


	return (IntersectionCount >0);
}

bool Collision::SphereIntersectPlane(Vector &planeNormal,const Vector &v1,const Vector &Center,const Vector &Velocity,float Radius,float &tMax){
	float t;
	float PlaneDistance = Matrix3DotProduct(&(VectorSubtraction(&Center,&v1)),&planeNormal) - Radius;
	float ProjRayLength = Matrix3DotProduct(&Velocity, &planeNormal);
 	if(PlaneDistance < 0.0f || ProjRayLength >-0.0000001f){
		if(ProjRayLength > -1e-5f) return false;
		if(PlaneDistance < -(Radius)) return false;
		tMax = ProjRayLength;

		return true;
	}
	//if(abs(ProjRayLength) < 1e-5f) return false;
	t = -(PlaneDistance/ProjRayLength);
	if(t < 0.0f || t > tMax) return false;
	//IntersectionPoint = VectorAddition(VectorMultiplication(VectorSubtraction(Direction,Origin),*t),Origin);
	//IntersectionPoint = VectorAddition(&(VectorMultiplication(&(VectorSubtraction(Velocity,Center)),t)),Center);
	tMax = t;

	return true;
}

bool Collision::SphereIntersectPoint(const Vector &Center, float Radius, const Vector &Velocity, const Vector &Point, float &tMax, Vector &CollisionNormal){
	Vector L, CollisionCenter;
	float a,b,c,l,l2,t;

	L = VectorSubtraction(&Center, &Point);
	l2 = VectorLength(L) * VectorLength(L);

	a = VectorLength(Velocity) * VectorLength(Velocity);
	b = 2.0f * Matrix3DotProduct(&Velocity, &L);
	c = l2 - (Radius*Radius);

	if(c < 0.0f){
		l = sqrtf(l2);
		t = l - Radius;
		
		if (tMax < t) return false;

		CollisionNormal = VectorMultiplication(&L, 1.0f/l);

		tMax = t;
		return true;
	}

	if (tMax < 0.0f) return false;

	if(!SolveCollision(a,b,c,t)) return false;

	if(t > tMax || t == 0.0f) return false;

	CollisionCenter = VectorAddition(&Center,&(VectorMultiplication(&Velocity,t)));

	Normalize(&CollisionNormal, &(VectorSubtraction(&CollisionCenter,&Point)));
	tMax = t;

	return true;
}

bool Collision::RayIntersectPlane(Triangle &testTriangle,Vector *Origin,Vector *Direction,float *t, Vector &IntersectionPoint){
	Plane testPlane;
	testPlane.a = testTriangle.v1.y*(testTriangle.v2.z - testTriangle.v3.z) + testTriangle.v2.y*(testTriangle.v3.z - testTriangle.v1.z) + testTriangle.v3.y*(testTriangle.v1.z - testTriangle.v2.z);
	testPlane.b = testTriangle.v1.z*(testTriangle.v2.x - testTriangle.v3.x) + testTriangle.v2.z*(testTriangle.v3.x - testTriangle.v1.x) + testTriangle.v3.z*(testTriangle.v1.x - testTriangle.v2.x);
	testPlane.c = testTriangle.v1.x*(testTriangle.v2.y - testTriangle.v3.y) + testTriangle.v2.x*(testTriangle.v3.y - testTriangle.v1.y) + testTriangle.v3.x*(testTriangle.v1.y - testTriangle.v2.y);
	testPlane.d = testTriangle.v1.x*(testTriangle.v2.y*testTriangle.v3.z-testTriangle.v3.y*testTriangle.v2.z)+testTriangle.v2.x*(testTriangle.v3.y*testTriangle.v1.z-testTriangle.v1.y*testTriangle.v3.z)+testTriangle.v3.x*(testTriangle.v1.y*testTriangle.v2.z-testTriangle.v2.y*testTriangle.v1.z);
	Vector planeNormal;
	planeNormal.x = testPlane.a;
	planeNormal.y = testPlane.b;
	planeNormal.z = testPlane.c;
	Normalize(&planeNormal,&planeNormal);
	float PlaneDistance = Matrix3DotProduct(&(VectorSubtraction(Origin,&(testTriangle.v1))),&planeNormal);
	float ProjRayLength = Matrix3DotProduct(&(VectorSubtraction(Direction,Origin)), &(VectorNegate(&planeNormal)));
	if(abs(ProjRayLength) < 1e-5f) return false;
	*t = PlaneDistance/ProjRayLength;
	if(*t < 0.0f || *t > 1.0f) return false;
	//IntersectionPoint = VectorAddition(VectorMultiplication(VectorSubtraction(Direction,Origin),*t),Origin);
	IntersectionPoint = VectorAddition(&(VectorMultiplication(&(VectorSubtraction(Direction,Origin)),*t)),Origin);
	return true;
}

bool Collision::PointInTriangle(Vector point,const Triangle &triangle){
	Vector edge, direction, edgeNormal;
	//First Edge
	edge = VectorSubtraction(&(triangle.v2), &(triangle.v1));
	direction = VectorSubtraction(&(triangle.v1), &point);
	edgeNormal = Matrix3CrossProduct(&edge, &(VectorComputeNormal(&(triangle.v1),&(triangle.v2),&(triangle.v3))));
	Normalize(&edgeNormal, &edgeNormal);
	if(Matrix3DotProduct(&direction, &edgeNormal)<0.0f) return false;
	//Second Edge
	edge = VectorSubtraction(&(triangle.v3), &(triangle.v2));
	direction = VectorSubtraction(&(triangle.v2), &point);
	edgeNormal = Matrix3CrossProduct(&edge, &(VectorComputeNormal(&(triangle.v1),&(triangle.v2),&(triangle.v3))));
	Normalize(&edgeNormal, &edgeNormal);
	if(Matrix3DotProduct(&direction, &edgeNormal)<0.0f) return false;
	//Third Edge
	edge = VectorSubtraction(&(triangle.v1), &(triangle.v3));
	direction = VectorSubtraction(&(triangle.v3), &point);
	edgeNormal = Matrix3CrossProduct(&edge, &(VectorComputeNormal(&(triangle.v1),&(triangle.v2),&(triangle.v3))));
	Normalize(&edgeNormal, &edgeNormal);
	if(Matrix3DotProduct(&direction, &edgeNormal)<0.0f) return false;

	return true;
}


bool Collision::SphereIntersectLineSegment(const Vector &Center, float Radius, Vector &Velocity,const Vector &v1,const Vector &v2, float &tMax, Vector &CollisionNormal){
	Vector E,D,x,y,PointOnEdge,CollisionCenter;
	float a,b,c,d,e,t,n;
	E = VectorSubtraction(&v2, &v1);
	D = VectorSubtraction(&Center, &v1);

	e = VectorLength(E);
	if(e<1e-5f) return false;
	E = VectorMultiplication(&E, 1.0f/e);

	

	x = Matrix3CrossProduct(&D, &E);
	y = Matrix3CrossProduct(&Velocity, &E);
	a= VectorLength(y) * VectorLength(y);
	b= 2.0f * (Matrix3DotProduct(&y,&x));
	c= (VectorLength(x) * VectorLength(x))-(Radius*Radius);
	
	if(c<0.0f){
		d = Matrix3DotProduct(&D, &E);
		if(d < 0.0f){
			return SphereIntersectPoint(Center,Radius,Velocity,v1,tMax,CollisionNormal);
		}else if(d > e){
			return SphereIntersectPoint(Center,Radius,Velocity,v2,tMax,CollisionNormal);
		}else{
			PointOnEdge = VectorAddition(&v1,&(VectorMultiplication(&E,d)));
			CollisionNormal = VectorSubtraction(&Center,&PointOnEdge);
			n = VectorLength(CollisionNormal);
			CollisionNormal = VectorMultiplication(&CollisionNormal, 1.0f/n);
			t = n-Radius;
			if(tMax < t) return false;
			tMax = t;

			return true;
		}
	}
	if(tMax < 0.0f) return false;
	if(!SolveCollision(a,b,c,t)) return false;
	if(t > tMax) return false;

	CollisionCenter = VectorAddition(&Center, &(VectorMultiplication(&Velocity,t)));
	d = Matrix3DotProduct(&(VectorSubtraction(&CollisionCenter,&v1)),&E);
	if(d<0.0f)
		return SphereIntersectPoint(Center,Radius,Velocity,v1,tMax,CollisionNormal);
	else if (d>e)
		return SphereIntersectPoint(Center,Radius,Velocity,v2,tMax,CollisionNormal);
	PointOnEdge = VectorAddition(&v1, &(VectorMultiplication(&E,d)));
	CollisionNormal = VectorSubtraction(&CollisionCenter,&PointOnEdge);
	Normalize(&CollisionNormal, &CollisionNormal);
	
	tMax = t;
	return true;
}


bool Collision::SolveCollision(float a, float b, float c, float &t){
	float d, one_over_two_a, t0,t1,temp;

	d = b*b - 4*a*c;

	if(d<0.0f) return false;

	d = sqrtf(d);
	one_over_two_a = 1.0f / (2.0f * a);

	t0 = (-b - d) * one_over_two_a;
	t1 = (-b + d) * one_over_two_a;

	if(t1 < t0){ temp = t0; t0=t1; t1 = temp;}

	if(t1 < 0.0f) return false;

	if(t0 < 0.0f) 
		t = t1;
	else 
		t = t0;
	return true;
}

bool Collision::SphereIntersectTriangle(const Vector &Center, float Radius, const Vector &Velocity,const Triangle &CollTriangle, float &tMax, Vector &CollisionNormal){
	float t = tMax;
	Vector CollisionCenter;
	bool bCollided = false;
	Plane testPlane;
	testPlane.a = CollTriangle.v1.y*(CollTriangle.v2.z - CollTriangle.v3.z) + CollTriangle.v2.y*(CollTriangle.v3.z - CollTriangle.v1.z) + CollTriangle.v3.y*(CollTriangle.v1.z - CollTriangle.v2.z);
	testPlane.b = CollTriangle.v1.z*(CollTriangle.v2.x - CollTriangle.v3.x) + CollTriangle.v2.z*(CollTriangle.v3.x - CollTriangle.v1.x) + CollTriangle.v3.z*(CollTriangle.v1.x - CollTriangle.v2.x);
	testPlane.c = CollTriangle.v1.x*(CollTriangle.v2.y - CollTriangle.v3.y) + CollTriangle.v2.x*(CollTriangle.v3.y - CollTriangle.v1.y) + CollTriangle.v3.x*(CollTriangle.v1.y - CollTriangle.v2.y);
	testPlane.d = CollTriangle.v1.x*(CollTriangle.v2.y*CollTriangle.v3.z-CollTriangle.v3.y*CollTriangle.v2.z)+CollTriangle.v2.x*(CollTriangle.v3.y*CollTriangle.v1.z-CollTriangle.v1.y*CollTriangle.v3.z)+CollTriangle.v3.x*(CollTriangle.v1.y*CollTriangle.v2.z-CollTriangle.v2.y*CollTriangle.v1.z);
	Vector triNormal;
	triNormal.x = testPlane.a;
	triNormal.y = testPlane.b;
	triNormal.z = testPlane.c;
	Normalize(&triNormal,&triNormal);
	//sphere intersect plane
	if(!SphereIntersectPlane(triNormal,CollTriangle.v1, Center, Velocity, Radius, t)) return false;
	//point in triangle
	if(t<0){
		CollisionCenter = VectorAddition(&Center, &(VectorMultiplication(&triNormal, -t)));
	}
	else{
		CollisionCenter = VectorAddition(&Center, &(VectorMultiplication(&Velocity,t)));
	}
	if(PointInTriangle(CollisionCenter,CollTriangle)){
		CollisionNormal = triNormal;
		tMax = t;
		return true;
	}

	bCollided |= SphereIntersectLineSegment(Center,Radius,VectorSubtraction(&Center,&Velocity),CollTriangle.v1,CollTriangle.v2,tMax,CollisionNormal);
	bCollided |= SphereIntersectLineSegment(Center,Radius,VectorSubtraction(&Center,&Velocity),CollTriangle.v2,CollTriangle.v3,tMax,CollisionNormal);
	bCollided |= SphereIntersectLineSegment(Center,Radius,VectorSubtraction(&Center,&Velocity),CollTriangle.v3,CollTriangle.v1,tMax,CollisionNormal);

	return bCollided;
}